function [out] = mcpp_vld_kalmfilt_e(in)
%MCPP_VLD_KALMFILT_E 蒙特卡罗仿真后处理
% FIXME: 陀螺零偏误差RMS变化异常，在1500s发散
% FIXME: 归一化P阵部分元素误差较大

dataDir = [pwd '\data\'];
for i=1:in.grpSimNum(1)
    load([dataDir '导航结果_1_' int2str(i) '.mat'], 'out_intNavError', 'auxOut_intNavError', 'out_inertNavIdeal', 'par_trajGen', 'par_intNav', 'iniCon_intNav');
    m = length(out_intNavError.t);
    n = size(out_intNavError.kalmFilt_tExtrap, 1);
    CHatEN = transpose_nd(out_intNavError.CNE);
    CEN = transpose_nd(out_inertNavIdeal.CNE);
    if i == 1
        load([dataDir '导航结果_1_' int2str(i) '.mat'], 'cst', 'iniCon_trajGen');
        posErr = NaN(m, 3, in.grpSimNum(1));
        vNErr = NaN(m, 3, in.grpSimNum(1));
        phi = NaN(m, 3, in.grpSimNum(1));
        dtheta = NaN(m, 3, in.grpSimNum(1));
        PxxTSum = zeros(37, 37, n);
        xTilde = NaN(n, 37, in.grpSimNum(1));
        % 每组蒙塔卡罗仿真kalmFilt_P都一样，任意取一组即可
        kalmFilt_P = auxOut_intNavError.kalmFilt_P;
    end
    accBiasErr = repmat(par_trajGen.IMU.acc.B', n, 1); % 加速度计的3个零偏误差
    accSFErr = repmat(par_trajGen.IMU.acc.dSF', n, 1); % 加速度计的3个标度因数误差
    accMAErr = repmat(offdiag2vec(par_trajGen.IMU.acc.MA)', n, 1); % 加速度计的6个失准角误差
    accSOErr = repmat(par_trajGen.IMU.acc.SO', n, 1); % 加速度计的3个二次项系数误差
    gyroBiasErr = repmat(par_trajGen.IMU.gyro.B', n, 1); % 陀螺的3个零偏误差
    gyroSFErr = repmat(par_trajGen.IMU.gyro.dSFp', n, 1); % 陀螺的3个标度因数误差
    gyroMAErr = repmat(offdiag2vec(par_trajGen.IMU.gyro.MA)', n, 1); % 陀螺的6个失准角误差
    posErr(:, :, i) = out_intNavError.pos - out_inertNavIdeal.pos;
    vNErr(:, :, i) = out_intNavError.vN - out_inertNavIdeal.vN;
    phi(:, :, i) = dcmdiff(out_intNavError.CBN, out_inertNavIdeal.CBN);
    dtheta(:, :, i) = dcmdiff(CHatEN, CEN);
    xTrue = [dtheta(1:10:end, 1:2, i) posErr(1:10:end, 3, i) vNErr(1:10:end, :, i) phi(1:10:end, :, i) dtheta(1:10:end, 3, i)...
        accBiasErr accSFErr accMAErr accSOErr gyroBiasErr gyroSFErr gyroMAErr];
    xHat = out_intNavError.kalmFilt_x;
    xTilde(:, :, i) = xHat - xTrue;
    for k=1:n
        PxxTSum(:, :, k) =  PxxTSum(:, :, k) + xTilde(k, :, i)'*xTilde(k, :, i);
    end
    disp(['完成第' int2str(i) '次仿真结果后处理']);
end
out.xTildeRMS = rms(xTilde, 3);
out.PxxTMean = PxxTSum / in.grpSimNum(1);
out.kalmFilt_P = kalmFilt_P;
% 系统误差
structNavPar = navparerrarr2struct(out.xTildeRMS(:, 1:10), [1:10 NaN(1, 5)], false);
plotnavparerr(out_intNavError.kalmFilt_tExtrap, structNavPar, {'误差RMS'}, [], [], [], '系统状态误差的RMS');
% 加速度计误差
structAccPar = acctriadpararr2struct(out.xTildeRMS(:, 11:25), [NaN(1, 3) 1:15], false);
plotacctriadpar(out_intNavError.kalmFilt_tExtrap, structAccPar, {'误差RMS'}, [], [], '加速度计状态误差的RMS');
% 陀螺误差
structGyroPar = gyrotriadpararr2struct(out.xTildeRMS(:, 26:37), [NaN(1, 3) 1:12 NaN(1, 9)], false);
plotgyrotriadpar(out_intNavError.kalmFilt_tExtrap, structGyroPar, {'误差RMS'}, [], [], '陀螺状态误差的RMS');
% 误差协方差矩阵
D = inv(sqrt(iniCon_intNav.kalmFilt.P));
DPErrD = NaN(37, 37, n);
PErr = out.kalmFilt_P - out.PxxTMean;
for k=1:n
    DPErrD(:, :, k) = D * PErr(:, :, k) * D; % 归一化
end
PErr_Max = reshape(max(max(abs(DPErrD))), n, 1);
preparefig('归一化P阵各元素误差的最大值');
plot(out_intNavError.kalmFilt_tExtrap, PErr_Max);
title('归一化P阵各元素误差的最大值');
xlabel('时间t');
plotsymmat(DPErrD, 3, '归一化P阵误差');
% PxxTMean和kalmFilt_P对角线元素的吻合度
preparefig('P阵对角线元素对比');
[m, n] = roundsqrt(37);
for i=1:37
    subplot(m, n, i);
    plot([sqrt(squeeze(out.PxxTMean(i, i, :))) sqrt(squeeze(out.kalmFilt_P(i, i, :)))]);
end
end